/**
* @file         chmotor_modbus_rtu.c
* @author       David Hu (hmd_hubei_cn@163.com)
* @brief         
* @version      0.1
* @date         2024.09.12
* @note          
* @copyright    Copyright (c) 2022 DAVID HU All rights reserved. Licensed under the MIT License (the "License");

* you may not use this file except in compliance with the License.

* You may obtain a copy of the License in the file LICENSE

* Unless required by applicable law or agreed to in writing, software

* distributed under the License is distributed on an "AS IS" BASIS,

* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

* See the License for the specific language governing permissions and

* limitations under the License.
**/

#include "chmotor_modbus_rtu.h"

static uint8_t gs_modbus_rtu_recv_databuff[128] = {0};
static uint8_t gs_modbus_rtu_recv_datalen = 0;
static uint8_t gs_modbus_rtu_send_packbuff[128] = {0};
static uint8_t gs_modbus_rtu_send_packlen = 0;

static enum {
  SLAVE_ADDR,
  FUNC_CODE,
  DATA_LEN,
  DATA
} gs_recv_fsm_state = SLAVE_ADDR;

static uint8_t gs_recv_dataindex = 0;
static uint8_t gs_recv_packlen = 0;
static uint8_t gs_slave_addr = 0;

static uint16_t modbus_crc16(uint8_t *buf_data, uint16_t buf_len) {
  uint16_t TCPCRC = 0xffff;
  uint16_t POLYNOMIAL = 0xa001;
  uint8_t i, j;

  for (i = 0; i < buf_len; i++) {
    TCPCRC ^= buf_data[i];
    for (j = 0; j < 8; j++) {
      if ((TCPCRC & 0x0001) != 0) {
        TCPCRC >>= 1;
        TCPCRC ^= POLYNOMIAL;
      } else {
        TCPCRC >>= 1;
      }
    }
  }
  return TCPCRC;
}

bool chmotor_modbus_read_register(uint8_t slave_addr, uint16_t start_reg_addr, uint16_t reg_num) {
  uint16_t crc16 = 0;
  uint8_t packlen = 0;
  gs_modbus_rtu_send_packbuff[0] = slave_addr;
  packlen++;
  gs_modbus_rtu_send_packbuff[1] = CHMOTOR_MODBUS_READ_REG_CMD;
  packlen++;
  gs_modbus_rtu_send_packbuff[2] = start_reg_addr >> 8;
  packlen++;
  gs_modbus_rtu_send_packbuff[3] = start_reg_addr & 0xff;
  packlen++;
  gs_modbus_rtu_send_packbuff[4] = reg_num >> 8;
  packlen++;
  gs_modbus_rtu_send_packbuff[5] = reg_num & 0xff;
  packlen++;
  crc16 = modbus_crc16(gs_modbus_rtu_send_packbuff, 6);
  gs_modbus_rtu_send_packbuff[6] = crc16 & 0xff;
  packlen++;
  gs_modbus_rtu_send_packbuff[7] = crc16 >> 8;
  packlen++;
  gs_modbus_rtu_send_packlen = packlen;

  return true;
}

bool chmotor_modbus_write_register(uint8_t slave_addr, uint16_t reg_addr, uint8_t* data, uint8_t data_len) {
  uint16_t crc16 = 0;
  uint16_t crc16_ltoh = 0;
  uint8_t packlen = 0;
  gs_modbus_rtu_send_packbuff[0] = slave_addr;
  packlen++;
  gs_modbus_rtu_send_packbuff[1] = CHMOTOR_MODBUS_WRITE_REG_CMD;
  packlen++;
  gs_modbus_rtu_send_packbuff[2] = reg_addr >> 8;
  packlen++;
  gs_modbus_rtu_send_packbuff[3] = reg_addr & 0xff;
  packlen++;

  if (data_len != 2) {
    return false;
  }

  for (int i = 0; i < data_len; i++) {
    gs_modbus_rtu_send_packbuff[4+i] = data[i];
    packlen++;
  }
  crc16 = modbus_crc16(gs_modbus_rtu_send_packbuff, 4+data_len);
  gs_modbus_rtu_send_packbuff[4+data_len] = crc16 & 0xff;
  packlen++;
  gs_modbus_rtu_send_packbuff[5+data_len] = crc16 >> 8;
  packlen++;
  gs_modbus_rtu_send_packlen = packlen;

  return true;
}

bool chmotor_modbus_write_multi_register(uint8_t slave_addr, uint16_t start_reg_addr, uint16_t reg_num, uint8_t* data, uint8_t data_len) {
  uint16_t crc16 = 0;
  uint16_t crc16_ltoh = 0;
  uint8_t packlen = 0;
  gs_modbus_rtu_send_packbuff[0] = slave_addr;
  packlen++;
  gs_modbus_rtu_send_packbuff[1] = CHMOTOR_MODBUS_WRITE_REG_CMD_MULTI;
  packlen++;
  gs_modbus_rtu_send_packbuff[2] = start_reg_addr >> 8;
  packlen++;
  gs_modbus_rtu_send_packbuff[3] = start_reg_addr & 0xff;
  packlen++;
  gs_modbus_rtu_send_packbuff[4] = reg_num >> 8;
  packlen++;
  gs_modbus_rtu_send_packbuff[5] = reg_num & 0xff;
  packlen++;
  gs_modbus_rtu_send_packbuff[6] = data_len;
  packlen++;

  if ((data_len + 9) > 128) {
    return false;
  }

  for (int i = 0; i < data_len; i++) {
    gs_modbus_rtu_send_packbuff[7+i] = data[i];
    packlen++;
  }
  crc16 = modbus_crc16(gs_modbus_rtu_send_packbuff, 7+data_len);
  gs_modbus_rtu_send_packbuff[7+data_len] = crc16 & 0xff;
  packlen++;
  gs_modbus_rtu_send_packbuff[8+data_len] = crc16 >> 8;
  packlen++;
  gs_modbus_rtu_send_packlen = packlen;

  return true;
}

uint8_t* chmotor_modbus_pack_get_packbuff(void) {
  return gs_modbus_rtu_send_packbuff;
}

uint8_t chmotor_modbus_pack_get_packlen(void) {
  return gs_modbus_rtu_send_packlen;
}

uint8_t chmotor_modbus_unpack_get_datalen(void) {
  return gs_modbus_rtu_recv_datalen;
}

uint8_t* chmotor_modbus_unpack_get_databuff(void) {
  return (uint8_t*)&gs_modbus_rtu_recv_databuff[3];
}

uint8_t chmotor_modbus_unpack_get_slave_addr(void) {
  return gs_slave_addr;
}

bool chmotor_modbus_parse_slave_ack_read(uint8_t in_data) {
  switch (gs_recv_fsm_state) {
    case SLAVE_ADDR: {
      if (CHMOTOR_DEV_SALVE_ADDR_M1 == in_data || \
          CHMOTOR_DEV_SALVE_ADDR_M2 == in_data || \
          CHMOTOR_DEV_SALVE_ADDR_M3 == in_data || \
          CHMOTOR_DEV_SALVE_ADDR_M4 == in_data || \
          CHMOTOR_DEV_SALVE_ADDR_M5 == in_data || \
          CHMOTOR_DEV_SALVE_ADDR_M6 == in_data || \
          CHMOTOR_DEV_SALVE_ADDR_M7 == in_data || \
          CHMOTOR_DEV_SALVE_ADDR_M8 == in_data) {
        gs_modbus_rtu_recv_databuff[gs_recv_dataindex++] = in_data;
        gs_recv_fsm_state = FUNC_CODE;
        gs_slave_addr = in_data;
      }
      break;
    }
    case FUNC_CODE: {
      if (CHMOTOR_MODBUS_READ_REG_CMD == in_data) {
        gs_modbus_rtu_recv_databuff[gs_recv_dataindex++] = in_data;
        gs_recv_fsm_state = DATA_LEN;
      } else {
        gs_recv_dataindex = 0;
        gs_recv_fsm_state = SLAVE_ADDR;
      }
      break;
    }
    case DATA_LEN: {
      if (in_data <= 16) {
        gs_recv_packlen = 3 + in_data + 2;
        gs_modbus_rtu_recv_databuff[gs_recv_dataindex++] = in_data;
        gs_recv_fsm_state = DATA;
      } else {
        gs_recv_dataindex = 0;
        gs_recv_fsm_state = SLAVE_ADDR;
      }
      break;
    }
    case DATA: {
      gs_modbus_rtu_recv_databuff[gs_recv_dataindex++] = in_data;
      if (gs_recv_dataindex >= gs_recv_packlen) {
        uint16_t crc16 = (gs_modbus_rtu_recv_databuff[gs_recv_dataindex-2] << 8) | gs_modbus_rtu_recv_databuff[gs_recv_dataindex-1];
        uint16_t check = modbus_crc16(gs_modbus_rtu_recv_databuff, gs_recv_packlen-2);
        gs_recv_dataindex = 0;
        gs_recv_fsm_state = SLAVE_ADDR;
        if (crc16 == check) {
          gs_modbus_rtu_recv_datalen = gs_modbus_rtu_recv_databuff[2];
          return true;
        } 
      }
      break;
    }
  }

  return false;
}


/********************* (C) COPYRIGHT DAVID HU *******END OF FILE ********/
